//
// Created by fl on 23-9-5.
//

#include "bodytraced_guidance.h"

bool BodyTracedGuidance::set_LOS_status(double vertical, double horizontal, double ver_rate, double hor_rate) {

    _blos_info.vertical = vertical;// vertical向下为正
    _blos_info.horizontal = horizontal;
    _blos_info.ver_rate = ver_rate;
    _blos_info.hor_rate = hor_rate;
}

bool BodyTracedGuidance::set_airspeed(double aspd) {
    _airspeed = aspd;
    return true;
}

bool BodyTracedGuidance::set_att_status(float roll, float pitch, float yaw) {
    _pitch = pitch;
    _roll = roll;
    _yaw = yaw;
//    std::cout << "\n" << "flying attitude : roll " << math::degrees(roll)  << ", pitch "
//    <<  math::degrees(pitch)  << ", yaw "
//    <<  math::degrees(yaw) << std::endl; ;
    return true;
}

bool BodyTracedGuidance::calc_att_thr_cmd(mavlink_set_attitude_target_t &cmd) {
    //set_airspeed();
    //set_att_status();
    float cmd_pitch;
    float cmd_roll;
    float cmd_yaw;
    float cmd_throttle;
    float TMG_VLOS_THRE = -10.0f;

    if ((float)_blos_info.vertical < math::radians(TMG_VLOS_THRE) &&
           abs((float)_blos_info.horizontal) < math::radians(10.f)) {
        _tmg_vlos_enable = true;
    }

    if (_tmg_vlos_enable) {
//        cmd_pitch = sdg_calc_pitch_IPG();
        cmd_pitch = sdg_calc_pitch_APG();
        cmd_throttle = 0.26f;
    } else {
        cmd_pitch = math::radians(1.8f);
        cmd_throttle = 0.3f;
    }

    cmd_roll = sdg_calc_roll();
    cmd_yaw = 0.0f;
//    cmd_throttle = sdg_calc_throttle();

    std::cout << "vertical blos " << math::degrees(_blos_info.vertical)
    << ", hor " << math::degrees(_blos_info.horizontal)
    << ", cmd roll " << math::degrees(cmd_roll)
    << ", roll " << math::degrees(_roll)
    << ", cmd pitch " << math::degrees(cmd_pitch)
    << ", pitch " << math::degrees(_pitch)
    << ", throttle " << cmd_throttle << std::endl;

    matrix::Eulerf euler(cmd_roll, cmd_pitch, cmd_yaw);
    matrix::Quatf cmd_q = euler;

    cmd.time_boot_ms = currentTimeMs();
    cmd.q[0] = cmd_q(0);
    cmd.q[1] = cmd_q(1);
    cmd.q[2] = cmd_q(2);
    cmd.q[3] = cmd_q(3);
    cmd.thrust = cmd_throttle;

    cmd.type_mask = 0b00000111;
}
// liyiliang daociyiyou
float BodyTracedGuidance::sdg_calc_pitch_APG() {
    float cmd_pitch;
    float sdg_vlos;
    float sdg_hlos;
    sdg_vlos = (float) _blos_info.vertical;
    sdg_hlos = (float) _blos_info.horizontal;

    cmd_pitch = -(float) sin(-_roll) * sdg_hlos + (float) cos(_roll) * sdg_vlos + _pitch;
    return cmd_pitch;
}

float BodyTracedGuidance::sdg_calc_pitch_IPG() {
    float cmd_pitch;
    float sdg_vlos;
    static float vlos_0 = 0.0f;
    static float pitch_0 = 0.0f;
    float SDG_IPG_N = 1.4f;
    float pitch_ec = 0.9f;

    sdg_vlos = (float) _blos_info.vertical;

    if (_init_finish) {
        vlos_0 = (float) _blos_info.vertical;
        pitch_0 = _pitch;
        _init_finish = false;
        std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!start to control pitch, dive to target!!!!!!!!!!!!!!!!!!!!!" << std::endl;
    }

    cmd_pitch = SDG_IPG_N * (sdg_vlos - vlos_0) + pitch_0;
    vlos_0 *= pitch_ec;
    return cmd_pitch;

}

float BodyTracedGuidance::sdg_calc_roll() {
    float cmd_roll;
    float sdg_vlos;
    float sdg_hlos;
    float sdg_roll;
    float roll_rate;
    float SDG_TC_R = 1.6f;

    sdg_vlos = (float) _blos_info.vertical;
    sdg_hlos = (float) _blos_info.horizontal;

    sdg_roll = (float)cos(-_roll) * sdg_hlos - (float)sin(-_roll) * sdg_vlos;
    roll_rate = (1/SDG_TC_R) * sdg_roll;
    cmd_roll = atanf((float )_airspeed/CONSTANTS_ONE_G * roll_rate);

    return math::constrain(cmd_roll, -math::radians(20.0f), math::radians(20.0f));
}

float BodyTracedGuidance::sdg_calc_throttle() {
    float cmd_throttle;
    cmd_throttle = 0.3f;
    return cmd_throttle;
}

mavlink_set_attitude_target_t BodyTracedGuidance::get_att_target() const {
    mavlink_set_attitude_target_t att_sp;
    std::cout << "get_att_target method not defined by bodyTracedGuidance"
              << std::endl;
    return att_sp;
}